#include "user.h"
#include "odometry.h"
#include "navigation.h"
#include <stdio.h>
#include <math.h>


int user_state = 0;
void user_tick_run(void) {

	static Position pos;
	if(user_state == 0) {
		pos.x = 0;
		pos.y = 0.8;
		pos.yaw = 0;
	}
	if(user_state == 1) {
		pos.x = -4.4;
	}
	if(user_state == 2) {
		pos.y = 4.8;
	}
	if(user_state == 3) {
		pos.x = -1.3;
		pos.yaw = 180;
	}
	if(user_state == 4) {
		pos.y = 3.8;
		pos.yaw = -90;
	}
	if(user_state <= 4) {
		user_state += navigate(&pos);
	}
	
}

// 校准数据
static float mag_min_x =  1e9, mag_max_x = -1e9;
static float mag_min_y =  1e9, mag_max_y = -1e9;
static int calibrating = 1;   // 是否正在校准

void mag_cali(void) {
    static int state = 0;

    if (calibrating) {
        // --- 更新采样最大最小值 ---
        if (odom.mx < mag_min_x) mag_min_x = odom.mx;
        if (odom.mx > mag_max_x) mag_max_x = odom.mx;
        if (odom.my < mag_min_y) mag_min_y = odom.my;
        if (odom.my > mag_max_y) mag_max_y = odom.my;
    }

    // --- 状态机 ---
    if (state == 0) {
        // 驱动小车旋转一圈
        state += drive_arc_by_angle(0.3, 360, 0.1);
    } 
    else if (state == 1 && calibrating) {
        // 一圈结束，停止校准
        calibrating = 0;

        // --- 计算 offset ---
        float offset_x = (mag_max_x + mag_min_x) / 2.0f;
        float offset_y = (mag_max_y + mag_min_y) / 2.0f;

        // --- 计算椭圆半径 ---
        float radius_x = (mag_max_x - mag_min_x) / 2.0f;
        float radius_y = (mag_max_y - mag_min_y) / 2.0f;

        // --- 椭圆拟合修正系数（归一化到平均半径） ---
        float avg_radius = (radius_x + radius_y) / 2.0f;
        float scale_x = avg_radius / radius_x;
        float scale_y = avg_radius / radius_y;

        // --- 输出最大最小值（原始采样） ---
        printf("Mag Sample Max/Min:\n");
        printf("mx: max=%d min=%d\n", (int)(mag_max_x*1000), (int)(mag_min_x*1000));
        printf("my: max=%d min=%d\n", (int)(mag_max_y*1000), (int)(mag_min_y*1000));

        // --- 输出校准参数（整数放大1000倍） ---
        printf("Mag Calib Result:\n");
        printf("offset_x=%d offset_y=%d\n", (int)(offset_x*1000), (int)(offset_y*1000));
        printf("scale_x=%d scale_y=%d\n",   (int)(scale_x*1000),  (int)(scale_y*1000));
    }
}
